perf(handrail_detect): cache model indices and hoist wTc.inverse() out of loops#832
Open
killerdevildog wants to merge 1 commit intonasa:developfrom
Open
Conversation
…t of loops Two targeted optimizations to SendRegistration(), called on every timer tick. Optimization 1: cache handrail model index list Before: every tick iterated ALL world models doing a string search on each name. After: RefreshHandrailModelIndices() builds a vector<size_t> of handrail indices once and rebuilds only when GetWorld()->GetModelCount() changes. Optimization 2: hoist wTc.inverse() out of both hot loops Before: wTc.inverse() called once per handrail candidate (scan loop) and once per ray sample (sampling loop) — same matrix decomposition recomputed ~2800 times per tick with default parameters. After: const Eigen::Affine3d cTw = wTc.inverse() computed once before both loops; all uses replaced with the cheaper cTw * x multiply. Benchmark (4000 world models, 800 handrail candidates, 2000 ray samples, 1000 iterations, -O3, Eigen 3, x86_64): Before: 42.2 us/tick After: 20.7 us/tick (~2x speedup, output checksums identical)
killerdevildog
force-pushed
the
perf/handrail-detect-hotpath-optimization
branch
from
1e73ee5 to
bc46952
Compare
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
What
Two small hot-path fixes in
GazeboSensorPluginHandrailDetect::SendRegistration():wTc.inverse()— was recomputed on every iteration of both the scan loop and thenum_sampsampling loop. Now computed once per tick.RefreshHandrailModelIndices()method caches the indices and only rescans whenModelCount()changes (i.e. once at startup).Why it matters
At the default config (
rate = 5 Hz,num_samp = 1000), the inverse hoist alone eliminates 1 000 redundant matrix decompositions per tick.Benchmark (synthetic stress: 4 000 models, 800 handrails,
num_samp = 1000):Output is numerically identical.
Scope
simulation/src/gazebo_sensor_plugin_handrail_detect/gazebo_sensor_plugin_handrail_detect.cc